// TrackBall.cpp
// 
#include "TrackBall.h"

#include "OpenGL/OpenGL.h"

#include "Geometry/Geometry.h"

#include <iostream>

using namespace OpenGL;

// ------------------------------------------------------------------------------------------------
TrackBall::TrackBall()
{
	reset();
}

// ------------------------------------------------------------------------------------------------
TrackBall::~TrackBall()
{}

// ------------------------------------------------------------------------------------------------
void TrackBall::reset()
{
	// no rotation
	setOrientation( Math::dmat4(1) );
	
	// center in the middle of the screen
	//_center = Math::dvec2(0.0);
	_center = Math::dvec3(0.0);
	_zoom   = 1;
	_radius = 0.8;
}

// ------------------------------------------------------------------------------------------------
void TrackBall::resize( int pixelWidth,int pixelHeight )
{
	if( pixelWidth  <= 1 ) pixelWidth  = 2;
	if( pixelHeight <= 1 ) pixelHeight = 2;

	//const int size = Math::min(pixelHeight,pixelWidth);

	// aspect ratio
	const double ar = pixelWidth / (double) pixelHeight;

	_size.x = 1.0f / ((pixelWidth) * 0.5f);
	_size.y = 1.0f / ((pixelHeight) * 0.5f);
	//_size.y = ar < 1.0 ? ar * _size.x : ar / _size.x;

	//_center = Math::dvec2(0.0);
	_center = Math::dvec3(0.0);
}

//void TrackBall::setCenter( const Math::ivec2 &screenPos )        { _center = normalized(screenPos); }
// ------------------------------------------------------------------------------------------------
Math::dvec3 TrackBall::eye()        const { return _eye;    }  
Math::dquat TrackBall::quaternion() const { return _currQ;  }
double TrackBall::radius()          const { return _radius; }
Math::dvec3 TrackBall::center()     const { return _center; }

// ------------------------------------------------------------------------------------------------
void TrackBall::setEye( const Math::dvec3 &eye )                 { _eye = eye; }
void TrackBall::setCenter( const Math::dvec3 &center )           { _center += center; }
void TrackBall::setOrientation( const Math::dmat4 &orientation ) { _currQ = Math::quat_cast(orientation);	_prevQ = _currQ; }
void TrackBall::setRadius( double radius )                       { _radius = radius; }
void TrackBall::setZoom( double zoom )                           { _zoom *= zoom;    }

// ------------------------------------------------------------------------------------------------
Math::dvec2 TrackBall::normalized( const Math::ivec2 &screenPos ) const
{
	// Adjust point coords and scale down to range of [-1 ... 1]
	return Math::dvec2( screenPos.x * _size.x - 1.0, screenPos.y * _size.y - 1.0 );	
}

// ------------------------------------------------------------------------------------------------
Math::dvec3 TrackBall::mapToSphere( const Math::ivec2 &screenPos ) const
{
	//const Math::dvec2 p = normalized( screenPos ) - _center;
	const Math::dvec2 p = normalized( screenPos );
		                 
	const double lenSqr = Math::dot(p,p);
	const double r2     = _radius * _radius;

	Math::dvec3 P;
	// sphere and hyperbolic sheet intersect on the circle
	//             r^2
	// x^2 + y^2 = ---
	//              2 
	if( lenSqr >= r2 * 0.5 )
	{
		// outside the sphere use hyperbolic sheet
		P = Math::dvec3(p.x,p.y, (r2 * 0.5) / Math::sqrt(lenSqr) );
		//std::cout << "Hyperbolic" << std::endl;
	}
	else
	{
		// inside the sphere
		P = Math::dvec3(p.x, p.y,Math::sqrt(r2-lenSqr));
		//std::cout << "Sphere" << std::endl;
	}

	return P;
}

// ------------------------------------------------------------------------------------------------
void TrackBall::startRubberBand( const Math::ivec2 &screenPos )
{
	_startPoint = mapToSphere(screenPos);
}

// ------------------------------------------------------------------------------------------------
void TrackBall::endRubberBand( const Math::ivec2 &screenPos )
{
	_prevQ = _currQ;
}

// ------------------------------------------------------------------------------------------------
void TrackBall::update( const Math::ivec2 &screenPos )
{
	// generate two vector on the sphere surface
	const Math::dvec3 currPoint   = mapToSphere( screenPos );
	
	// compute the rotation axis
	const Math::dvec3 cross = Math::cross( _startPoint,currPoint );

	if( Math::length(cross) < 1e-5 )
		_currQ = Math::dquat();
	else
	{
		_currQ = Math::dquat( Math::dot(_startPoint,currPoint), cross );
		_currQ = Math::normalize( _currQ );
	}
	
	if( Math::length(_prevQ) > 0.0 )
		_currQ *= _prevQ;
}

// ------------------------------------------------------------------------------------------------
Math::dmat4 TrackBall::matrix() const 
{ 
	const Math::dmat4 r = Math::mat4_cast( _currQ ); 
	const Math::dmat4 s = Math::scale( Math::dmat4(1.0), Math::dvec3(_zoom) );
	const Math::dmat4 t = Math::translate( Math::dmat4(1.0), _center );
	return r*s*t; 
}

// ------------------------------------------------------------------------------------------------
void TrackBall::draw() const
{
	glPushAttrib( GL_LINE_BIT | GL_CURRENT_BIT );
	glLineWidth(1.0);

	glPushMatrix();
	glLoadIdentity();
	double z = _radius/Math::tan( Math::radians(45.*0.5));
	glTranslated( 0, 0, -z*1.05 );
	glMultMatrixd( Math::value_ptr(Math::mat4_cast( _currQ )) );

	const std::vector<Math::dvec3> pts = Geometry::circle( Math::dvec3(0),_radius );

	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer( 3, GL_DOUBLE, 0, &pts[0] );
	
	glPushMatrix();
	glColor4ub(255,128,128,64);
	glRotatef(0,0,0,1);
	glDrawArrays( GL_LINE_STRIP, 1, pts.size()-1 );
	glPopMatrix();

	glPushMatrix();
	glColor4ub(128,255,128,64);
	glRotatef(90,0,1,0);
	glDrawArrays( GL_LINE_STRIP, 1, pts.size()-1 );
	glPopMatrix();

	glPushMatrix();
	glColor4ub(128,128,255,64);
	glRotatef(90,1,0,0);
	glDrawArrays( GL_LINE_STRIP, 1, pts.size()-1 );
	glPopMatrix();

	glPopMatrix();

	glDisableClientState(GL_VERTEX_ARRAY);
	glPopAttrib();
}